#include <ipa_room_segmentation/voronoi_segmentation.h>
#include <ipa_room_segmentation/wavefront_region_growing.h>
#include <ipa_room_segmentation/contains.h>
#include <ipa_room_segmentation/timer.h>
#include <set>

AbstractVoronoiSegmentation::AbstractVoronoiSegmentation() {}

bool AbstractVoronoiSegmentation::determineRoomIndexFromRoomID(
        const std::vector<Room> &rooms, const int room_id, size_t &room_index)
{
    bool found_id = false;
    for (size_t r = 0; r < rooms.size(); r++)
    {
        if (rooms[r].getID() == room_id)
        {
            room_index = r;
            found_id = true;
            break;
        }
    }
    return found_id;
}

// 合并房间对
void AbstractVoronoiSegmentation::mergeRoomPair(std::vector<Room> &rooms, const int target_index,
                                                const int room_to_merge_index, cv::Mat &segmented_map,
                                                const double map_resolution)
{
    // integrate room to merge into target and delete merged room
    const int target_id = rooms[target_index].getID();
    const int room_to_merge_id = rooms[room_to_merge_index].getID();
    rooms[target_index].mergeRoom(rooms[room_to_merge_index], map_resolution);
    rooms[room_to_merge_index].setRoomId(rooms[target_index].getID(), segmented_map);
    rooms.erase(rooms.begin() + room_to_merge_index);
    std::sort(rooms.begin(), rooms.end(), sortRoomsAscending);
    // update neighborhood statistics for remaining rooms
    for (auto & room : rooms)
    {
        std::vector<int> &neighbor_ids = room.getNeighborIDs();
        auto it = std::find(neighbor_ids.begin(), neighbor_ids.end(), room_to_merge_id);
        if (it != neighbor_ids.end())
        {
            auto it2 = std::find(neighbor_ids.begin(), neighbor_ids.end(), target_id);
            if (it2 != neighbor_ids.end()) neighbor_ids.erase(it);
            else *it = target_id;
        }
        std::map<int, int> &neighbor_statistics = room.getNeighborStatistics();
        auto it3 = neighbor_statistics.find(room_to_merge_id);
        if (it3 != neighbor_statistics.end())
        {
            auto it4 = neighbor_statistics.find(target_id);
            if (it4 != neighbor_statistics.end()) it4->second += it3->second;
            else neighbor_statistics[target_id] = it3->second;
            neighbor_statistics.erase(it3);
        }
    }
}

void AbstractVoronoiSegmentation::drawVoronoi(cv::Mat &img,
                                              const std::vector<std::vector<cv::Point2f> > &facets_of_voronoi,
                                              const cv::Scalar voronoi_color, const cv::Mat &eroded_map)
{
    // go trough each facet of the calculated Voronoi-graph and check if it should be drawn.
    // 遍历计算出的Voronoi-graph的每个面并检查是否应该绘制它。
    for (const auto & current_contour : facets_of_voronoi)
    {
        // saving-variable for the last Point that has been looked at 保存最后查看的点的变量
        cv::Point2f last_point = current_contour.back();
        // draw each line of the voronoi-cell 绘制voronoi细胞的每条线
        for (auto current_point : current_contour)
        {
            // variable to check, whether a Point is inside a white area or not 变量来检查一个点是否在白色区域内
            bool inside = true;
            // only draw lines that are inside the map-contour 只绘制地图等高线内部的线
            if (((int) current_point.x < 0) || ((int) current_point.x >= eroded_map.cols) ||
                ((int) current_point.y < 0) || ((int) current_point.y >= eroded_map.rows) ||
                eroded_map.at<uchar>((int) current_point.y, (int) current_point.x) == 0 ||
                ((int) last_point.x < 0) || ((int) last_point.x >= eroded_map.cols) ||
                ((int) last_point.y < 0) || ((int) last_point.y >= eroded_map.rows) ||
                eroded_map.at<uchar>((int) last_point.y, (int) last_point.x) == 0)
                inside = false;
            if (inside) cv::line(img, last_point, current_point, voronoi_color, 1);
            last_point = current_point;
        }
    }
}

//**************** Create the Generalized Voronoi-Diagram **********************
/* 此函数用于在给定的地图中创建一般化的voronoi图。它执行以下步骤:
 This function is here to create the generalized voronoi-graph in the given map. It does following steps:
	1. It finds every discretized contour in the given map (they are saved as vector<Point>).
     Then it takes these contour-Points and adds them to the OpenCV Delaunay generator from which the voronoi-cells can be generated.
	2. Finally it gets the boundary-Points of the voronoi-cells with getVoronoiFacetList.
	   It takes these facets and draws them using the drawVoronoi function.
	   This function draws the facets that only have Points inside the map-contour
	   (other lines go to not-reachable places and are not necessary to be looked at).
	3. It returns the map that has the generalized voronoi-graph drawn in.

 1。它在给定的地图中找到每个离散的轮廓(它们被保存为向量<点>)，然后将这些轮廓点添加到OpenCV Delaunay生成器中，从而生成voronoi细胞。
 2。最后利用getVoronoiFacetList方法得到voronoi细胞的边界点，使用drawVoronoi函数绘制这些面。
    这个函数绘制在地图等高线内只有点的切面(其他行到不可到达的地方，不需要查看)。
 3。它返回绘制了广义voronoi图的映射。
 */
void AbstractVoronoiSegmentation::createVoronoiGraph(cv::Mat &map_for_voronoi_generation, bool display_map)
{
    // variable to save the given map for drawing in the voronoi-diagram
    cv::Mat map_to_draw_voronoi_in = map_for_voronoi_generation.clone();
    // variable to save the given map in the createVoronoiGraph-function
    // 变量将给定的地图保存在createVoronoiGraph函数中
    cv::Mat temporary_map_to_calculate_voronoi = map_for_voronoi_generation.clone();
    // apply a closing-operator on the map so bad parts are neglected
    cv::erode(temporary_map_to_calculate_voronoi, temporary_map_to_calculate_voronoi, cv::Mat());
    cv::dilate(temporary_map_to_calculate_voronoi, temporary_map_to_calculate_voronoi, cv::Mat());
    //******************** 1. Get OpenCV delaunay-traingulation ******************************
    // variables to generate the voronoi-diagram, using OpenCVs delaunay-triangulation
    // 使用OpenCV的delaunay三角剖分生成voronoi图的变量,rect函数：Rect（左上角x坐标 ， 左上角y坐标，矩形的宽，矩形的高）
    cv::Rect rect(0, 0, map_to_draw_voronoi_in.cols, map_to_draw_voronoi_in.rows);
    cv::Subdiv2D subdiv(rect);
    // variable to save the hole-contours (= black holes inside the white map)
    // 保存黑洞轮廓的变量(=白色地图中的黑洞)
    std::vector<std::vector<cv::Point> > hole_contours;
    // variables for contour extraction and discretisation
    std::vector<std::vector<cv::Point> > contours;
    // hierarchy saves if the contours are hole-contours 如果轮廓是孔洞轮廓，则保存层次结构:
    // hierarchy[{0,1,2,3}]={next contour (same level), previous contour (same level), child contour, parent contour}
    // child-contour = 1 if it has one, = -1 if not, same for parent_contour
    // hierarchy[{0,1,2,3}]={下一个轮廓(相同级别)，上一个轮廓(相同级别)，子轮廓，父轮廓}
    // 如果有child-contour = 1，如果没有= -1,parent_contour也一样
    std::vector<cv::Vec4i> hierarchy;
    // get contours of the map
    cv::Mat temp = map_to_draw_voronoi_in.clone();
#if CV_MAJOR_VERSION <= 3
    cv::findContours(temp, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE);
    cv::drawContours(map_to_draw_voronoi_in, contours, -1, cv::Scalar(255), CV_FILLED);
#else
    cv::findContours(temp, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE);
    cv::drawContours(map_to_draw_voronoi_in, contours, -1, cv::Scalar(255), cv::FILLED);
#endif
    // put every point of the map-contours into the Delaunay-generator of OpenCV
    // 将地图等高线的每个点放入OpenCV的delaunay生成器
    for (int current_contour = 0; current_contour < contours.size(); current_contour++)
    {
        for (auto fp : contours[current_contour]) subdiv.insert(fp);
        // get the contours of the black holes --> it is necessary to check if points are inside these in drawVoronoi
        // 获取黑洞的轮廓——>在drawVoronoi中检查点是否在黑洞内部是必要的
        if (hierarchy[current_contour][2] == -1 && hierarchy[current_contour][3] != -1)
        {
            hole_contours.push_back(contours[current_contour]);
        }
    }
    //******************** 2. Get facets and draw voronoi-Graph ******************************
    // erode the map so that points near the boundary are not drawn later (see drawVoronoi)
    // 侵蚀地图，使边界附近的点以后不会被绘制(见drawVoronoi)
    cv::Mat eroded_map;
    cv::Point anchor(-1, -1);
    cv::erode(temporary_map_to_calculate_voronoi, eroded_map, cv::Mat(), anchor, 2);
    // get the Voronoi regions from the delaunay-subdivision graph
    // define the voronoi-drawing colour
    const cv::Scalar voronoi_color(127);
    // variables to find the facets and centers of the voronoi-cells
    // 变量查找voronoi细胞的面和中心
    std::vector<std::vector<cv::Point2f> > voronoi_facets;
    std::vector<cv::Point2f> voronoi_centers;
    // 获得所有Voronoi facet的列表。
    subdiv.getVoronoiFacetList(std::vector<int>(), voronoi_facets, voronoi_centers);
    // draw the voronoi-regions into the map
    drawVoronoi(map_to_draw_voronoi_in, voronoi_facets, voronoi_color, eroded_map);
    if (display_map)
    {
        imshow("map_to_draw_voronoi_in", map_to_draw_voronoi_in);
        cv::waitKey(1);
    }
    // make pixels black, which were black before and were colored by the voronoi-regions
    // 使像素黑色，之前是黑色的，并通过voronoi区域着色 v0e9
    for (int v = 0; v < map_to_draw_voronoi_in.rows; v++)
    {
        for (int u = 0; u < map_to_draw_voronoi_in.cols; u++)
        {
            if (map_for_voronoi_generation.at<unsigned char>(v, u) == 0)
            {
                map_to_draw_voronoi_in.at<unsigned char>(v, u) = 0;
            }
        }
    }
    map_for_voronoi_generation = map_to_draw_voronoi_in;
}

void AbstractVoronoiSegmentation::pruneVoronoiGraph(
        cv::Mat &voronoi_map, std::set<cv::Point, cv_Point_comp> &node_points)
{
    // 1.extract the node-points that have at least three neighbors on the voronoi diagram
    // node-points are points on the voronoi-graph that have at least 3 neighbors
    // 1。提取voronoi图上至少有三个邻居的节点点
    for (int v = 1; v < voronoi_map.rows - 1; v++)
    {
        for (int u = 1; u < voronoi_map.cols - 1; u++)
        {
            if (voronoi_map.at<unsigned char>(v, u) == 127)
            {
                // variable to save the number of neighbors for each point
                // 变量保存每个点的邻居数量
                int neighbor_count = 0;
                // check 3x3 region around current pixel 检查当前像素周围的3x3区域
                for (int row_counter = -1; row_counter <= 1; row_counter++)
                {
                    for (int column_counter = -1; column_counter <= 1; column_counter++)
                    {
                        // don't check the point itself 不要检查要点本身
                        if (row_counter == 0 && column_counter == 0) continue;
                        // check if neighbors are colored with the voronoi-color 检查邻居是否用voronoi颜色着色
                        if (voronoi_map.at<unsigned char>(v + row_counter, u + column_counter) == 127) neighbor_count++;
                    }
                }
                if (neighbor_count > 2) node_points.insert(cv::Point(u, v));
            }
        }
    }
    // 2.reduce the side-lines along the voronoi-graph by checking if it has only one neighbor until a node-point is reached
    // 2。通过检查voronoi图是否只有一个邻居来减少沿voronoi图的边线，直到到达一个节点点
    //	--> make it white 设置为白色
    //	repeat a large enough number of times so the graph converges 重复足够多的次数，使图形收敛
    for (int step = 0; step < 100; step++)
    {
        for (int v = 0; v < voronoi_map.rows; v++)
        {
            for (int u = 0; u < voronoi_map.cols; u++)
            {
                // set that the point is a point along the graph and not a side-line
                // 设置该点是沿图形的点，而不是边线
                if (voronoi_map.at<unsigned char>(v, u) == 127)
                {
                    // variable to save the number of neighbors for each point
                    int neighbor_count = 0;
                    for (int row_counter = -1; row_counter <= 1; row_counter++)
                    {
                        for (int column_counter = -1; column_counter <= 1; column_counter++)
                        {
                            // don't check the point itself
                            if (row_counter == 0 && column_counter == 0) continue;
                            // check the surrounding points
                            const int nv = v + row_counter;
                            const int nu = u + column_counter;
                            if (nv >= 0 && nu >= 0 && nv < voronoi_map.rows && nu < voronoi_map.cols &&
                                voronoi_map.at<unsigned char>(nv, nu) == 127) neighbor_count++;
                        }
                    }
                    // if the current point is a node point found in the previous step, it belongs to the voronoi-graph
                    // 如果当前点是在前一步中找到的节点点，则属于voronoi-graph
                    if (neighbor_count <= 1 && node_points.find(cv::Point(u, v)) == node_points.end())
                    {
                        // if the Point isn't on the voronoi-graph make it white
                        // 如果Point不在voronoi-graph上，则将其设置为白色
                        voronoi_map.at<unsigned char>(v, u) = 255;
                    }
                }
            }
        }
    }
}

void AbstractVoronoiSegmentation::mergeRooms(cv::Mat &map_to_merge_rooms, std::vector<Room> &rooms,
                                             double map_resolution_from_subscription, double max_area_for_merging,
                                             bool display_map)
{
    // 该函数从原始voronoi分割算法中获取分割后的Map，并将房间合并在一起;
    // This function takes the segmented Map from the original Voronoi-segmentation-algorithm and merges rooms together,
    // that are small enough and have only two or one neighbor. 它们足够小，只有两个或一个邻居。
    // 1. go trough every pixel and add points to the rooms with the same ID
    // 1。遍历每个像素，并为具有相同ID的房间添加点
    for (int y = 0; y < map_to_merge_rooms.rows; y++)
    {
        for (int x = 0; x < map_to_merge_rooms.cols; x++)
        {
            int current_id = map_to_merge_rooms.at<int>(y, x);
            if (current_id != 0)
            {
                // add the Points with the same Id as a room to it 添加与房间相同Id的点到它
                for (auto & room : rooms)
                {
                    // insert the current point into the corresponding room 将当前点插入相应的房间
                    if (room.getID() == current_id)
                    {
                        room.insertMemberPoint(cv::Point(x, y), map_resolution_from_subscription);
                        break;
                    }
                }
            }
        }
    }
    // 2. add the neighbor IDs for every point 添加每个点的邻居id
    for (auto & room : rooms)
    {
        const int current_id = room.getID();
        // storage for already counted neighborhood points 存储已计算的邻域点
        std::vector<cv::Point> considered_neighbors;
        const std::vector<cv::Point> &current_points = room.getMembers();
        for (const auto & current_point : current_points)
        {
            for (int row_counter = -1; row_counter <= 1; row_counter++)
            {
                for (int col_counter = -1; col_counter <= 1; col_counter++)
                {
                    const int label = map_to_merge_rooms.at<int>(current_point.y + row_counter,
                                                                 current_point.x + col_counter);
                    // collect neighbor IDs 收集邻居id
                    if (label != 0 && label != current_id) room.addNeighborID(label);
                    // neighborhood statistics
                    cv::Point neighbor_point(current_point.x + col_counter,
                                             current_point.y + row_counter);
                    if (!contains(considered_neighbors, neighbor_point) && label != current_id)
                    {
                        room.addNeighbor(label);
                        considered_neighbors.push_back(neighbor_point);
                    }
                }
            }
        }
    }
    // 3. merge criteria 合并条件
    // sort rooms ascending by area 按面积排序房间
    std::sort(rooms.begin(), rooms.end(), sortRoomsAscending);
    // a) rooms with one neighbor and max. 75% walls around 只有一个邻居的房间。75%四周有墙
    for (int current_room_index = 0; current_room_index < rooms.size();)
    {
        Room &current_room = rooms[current_room_index];
        bool merge_rooms = false;
        size_t merge_index = 0;
        if (current_room.getNeighborCount() == 1 &&
            current_room.getArea() < max_area_for_merging &&
            current_room.getWallToPerimeterRatio() <= 0.75)
        {
            // check every room if it should be merged with its neighbor that it shares the most boundary with
            // 检查每个房间是否应该与相邻的房间合并
            merge_rooms = determineRoomIndexFromRoomID(rooms,
                    current_room.getNeighborWithLargestCommonBorder(), merge_index);
        }
        if (merge_rooms)
        {
            //std::cout << "merge " << current_room.getCenter() << ", id=" << current_room.getID() << " into "
            //          << rooms[merge_index].getCenter() << ", id=" << rooms[merge_index].getID() << std::endl;
            mergeRoomPair(rooms, merge_index, current_room_index, map_to_merge_rooms, map_resolution_from_subscription);
            current_room_index = 0;
        }
        else current_room_index++;
    }
    if (display_map) cv::imshow("a", map_to_merge_rooms);
    // b) small rooms
    for (int current_room_index = 0; current_room_index < rooms.size();)
    {
        Room &current_room = rooms[current_room_index];
        bool merge_rooms = false;
        size_t merge_index = 0;
        const int max_border_neighbor_id = current_room.getNeighborWithLargestCommonBorder();
        if (current_room.getArea() < 2.0 &&
            (double) current_room.getNeighborStatistics()[max_border_neighbor_id] / current_room.getPerimeter() > 0.2)
        {
            // merge with that neighbor that shares the most neighboring pixels
            merge_rooms = determineRoomIndexFromRoomID(rooms, max_border_neighbor_id, merge_index);
            if ((double) rooms[merge_index].getWallToPerimeterRatio() > 0.8) //0.8
                merge_rooms = false;
        }
        if (merge_rooms)
        {
            //std::cout << "merge " << current_room.getCenter() << ", id=" << current_room.getID() << " into "
            //          << rooms[merge_index].getCenter() << ", id=" << rooms[merge_index].getID() << std::endl;
            mergeRoomPair(rooms, merge_index, current_room_index, map_to_merge_rooms, map_resolution_from_subscription);
            current_room_index = 0;
        }
        else current_room_index++;
    }
    if (display_map) cv::imshow("b", map_to_merge_rooms);
    // c) merge a room with one neighbor that has max. 2 neighbors and sufficient wall ratio (connect parts inside a room)
    for (int current_room_index = 0; current_room_index < rooms.size();)
    {
        Room &current_room = rooms[current_room_index];
        bool merge_rooms = false;
        size_t merge_index = 0;
        // merge a room with one neighbor that has max. 2 neighbors and sufficient wall ratio (connect parts inside a room)
        const int max_border_neighbor_id = current_room.getNeighborWithLargestCommonBorder();
        if ((current_room.getNeighborCount() == 1 || current_room.getPerimeterRatioOfXLargestRooms(1) > 0.98) &&
            current_room.getWallToPerimeterRatio() > 0.5 &&
            (double) current_room.getNeighborStatistics()[max_border_neighbor_id] / current_room.getPerimeter() > 0.15)
        {
            // merge with that neighbor that shares the most neighboring pixels
            merge_rooms = determineRoomIndexFromRoomID(rooms, max_border_neighbor_id, merge_index);
            if (rooms[merge_index].getNeighborCount() > 2 &&
                rooms[merge_index].getPerimeterRatioOfXLargestRooms(2) < 0.95)
                // || rooms[merge_index].getWallToPerimeterRatio() < 0.4)
                merge_rooms = false;
        }
        if (merge_rooms)
        {
            //std::cout << "merge " << current_room.getCenter() << ", id=" << current_room.getID() <<
            //  " into " << rooms[merge_index].getCenter() << ", id=" << rooms[merge_index].getID() << std::endl;
            mergeRoomPair(rooms, merge_index, current_room_index, map_to_merge_rooms, map_resolution_from_subscription);
            current_room_index = 0;
        }
        else current_room_index++;
    }
    if (display_map) cv::imshow("c", map_to_merge_rooms);
    // d) merge rooms that share a significant part of their perimeter
    for (int current_room_index = 0; current_room_index < rooms.size();)
    {
        Room &current_room = rooms[current_room_index];
        bool merge_rooms = false;
        size_t merge_index = 0;
        // common border length, room_id
        std::map<int, int, std::greater<int> > neighbor_room_statistics_inverse;
        current_room.getNeighborStatisticsInverse(neighbor_room_statistics_inverse);
//		std::vector<int>& neighbor_ids = current_room.getNeighborIDs();
//		for (size_t n=0; n<neighbor_ids.size(); ++n)
        for (auto & it : neighbor_room_statistics_inverse)
        {
            // skip wall
            if (it.second == 0) continue;
            const double neighbor_border_ratio =
                    (double) current_room.getNeighborStatistics()[it.second] / current_room.getPerimeter();
            if (neighbor_border_ratio > 0.2 ||
               (neighbor_border_ratio > 0.1 &&
                current_room.getWallToPerimeterRatio() > (1 - 2 * neighbor_border_ratio - 0.05) &&
                current_room.getWallToPerimeterRatio() < (1 - neighbor_border_ratio)))
            {
                // merge with that neighbor that shares the most neighboring pixels
                // 与共享最多相邻像素的邻居合并
                merge_rooms = determineRoomIndexFromRoomID(rooms, it.second, merge_index);
                if ((double) rooms[merge_index].getNeighborStatistics()[current_room.getID()] /
                    rooms[merge_index].getPerimeter() <= 0.1) merge_rooms = false;
                if (merge_rooms) break;
            }
        }
        if (merge_rooms)
        {
            //std::cout << "merge " << current_room.getCenter() << ", id=" << current_room.getID() <<
            //      " into " << rooms[merge_index].getCenter() << ", id=" << rooms[merge_index].getID() << std::endl;
            mergeRoomPair(rooms, merge_index, current_room_index, map_to_merge_rooms, map_resolution_from_subscription);
            current_room_index = 0;
        }
        else current_room_index++;
    }
    if (display_map) cv::imshow("d", map_to_merge_rooms);
    // e) largest room neighbor touches > 0.5 perimeter (happens often with furniture)
    // e） 最大的房间邻居接触> 0.5 周长（经常发生在家具上）
    for (int current_room_index = 0; current_room_index < rooms.size();)
    {
        Room &current_room = rooms[current_room_index];
        bool merge_rooms = false;
        size_t merge_index = 0;
        const int max_border_neighbor_id = current_room.getNeighborWithLargestCommonBorder();
        if ((double) current_room.getNeighborStatistics()[max_border_neighbor_id] / current_room.getPerimeter() > 0.4)
        {
            // merge with that neighbor that shares the most neighboring pixels
            // 与共享最多相邻像素的邻居合并
            merge_rooms = determineRoomIndexFromRoomID(rooms, max_border_neighbor_id, merge_index);
        }
        if (merge_rooms)
        {
            //std::cout << "merge " << current_room.getCenter() << ", id=" << current_room.getID() <<
            //     " into " << rooms[merge_index].getCenter() << ", id=" << rooms[merge_index].getID() << std::endl;
            mergeRoomPair(rooms, merge_index, current_room_index, map_to_merge_rooms, map_resolution_from_subscription);
            current_room_index = 0;
        }
        else current_room_index++;
    }
}
